package behaviours;

import core.Actuators;
import core.Senses;

public class FollowWall extends Behaviour {

	private static final int MAX_PROXIMITY = 700;
	private static final int MIN_PROXIMITY = 400;
	private static final int BASE_PROXIMITY = 550;
	private static final int NOISE_TOLERANCE = 20;
	
	@Override
	protected boolean isFinished() {
		Actuators.setMotors(0,0);
		Senses.readGround();
		Senses.readProximity();
		return(Senses.isOnGround() || (Senses.getProximity(3) > 80) || (Senses.getProximity(5) < NOISE_TOLERANCE) );
	}
	int XXX = 0;

	@Override
	protected void begin() throws InterruptedException {
		turn = 0;
		System.out.println(">>>"+ ++XXX + " ###########################################");
	}


	@Override
	protected void end() throws InterruptedException {

		System.out.println("<<<"+ XXX + " ###########################################");
	}
	
	int turn = 0;
	@Override
	protected void loop() throws InterruptedException {
		int left = 4, right = 4;
		float x = Senses.getProximity(5);
		System.out.print("TURN: "+turn);
		if(x > MAX_PROXIMITY){
			if(turn != -1){
				right += 1;
				turn = -1;
			}
			
		}else if(x < MIN_PROXIMITY){
			if(turn != 1){
				left += 1;
				turn = 1;
			}

		}else if(turn != 0){
			if(turn == 1)
				right += 1;
			else
				left += 1;
			turn = 0;
		}

		System.out.println(" BASE: "+x+" SPEED: "+ Math.round(left)+" "+Math.round(right)+" "+Senses.getProximity(5)+" "+Senses.getProximity(3)+" "+Senses.getProximity(2)+" newTurn: "+turn);
		Actuators.setMotors(left, right);
		
		
		
	}
	

}
